/*
 * NXT ajuri. Lukee sijaintitiedot BT:ltä. Suorittaa mittaukset ja lähettää
 * BT:llä tiedot takaisin tietokoneelle. Pitää sijaintitiedot moottoreiden
 * liikkeen perusteella. Kannattaa lukea BT-paketti.java
 */

package Main;

import java.awt.geom.Point2D;
import lejos.geom.Point;
import lejos.nxt.*;
import lejos.nxt.addon.OpticalDistanceSensor;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.localization.PoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.NavPathController;
import lejos.robotics.navigation.Pose;

public class Robo {
    /*Mitat on annettu millimetreinä.*/
    private final int tire_diameter = 42;
    private final int vehicle_width = 140; //keskeltä keskelle
    private final int vehicle_length = 235;
    
    private DifferentialPilot pilotti;
    private PoseProvider asettaja;
    private NavPathController navigaattori;
    private Tutka tutka;
    
    public Robo(){    
        LCD.clear();
        
        //luodaan liikuttamiseen tarvittavat osat
        pilotti = new DifferentialPilot(tire_diameter,
                vehicle_width, Motor.A, Motor.C);
        asettaja = new OdometryPoseProvider(pilotti);
        navigaattori = new NavPathController(pilotti, asettaja);
        
        //alustetaan muuttujat tutkaa varten
        NXTRegulatedMotor moottori = Motor.B;
        OpticalDistanceSensor[] sensorit = {new OpticalDistanceSensor(SensorPort.S1), 
            new OpticalDistanceSensor(SensorPort.S4)};
        
        tutka = new Tutka(moottori, sensorit);
    }
    
    public BTPaketti liikuJaMittaa(BTPaketti paketti){        
        // Lue paketista robotin uusi sijainti, jonka tietokone on laskenut
        // vanhojen mittaustulosten perusteella.
        asettaja.setPose(new Pose(paketti.getNykySijainti().x,
                                  paketti.getNykySijainti().y,
                                  asettaja.getPose().getHeading()));
        
        // Liiku uuteen mittauspisteeseen.
        navigaattori.goTo(paketti.getUusiSijainti().getX(),
                          paketti.getUusiSijainti().getY());

        // Käänny katsomaan mittasupistettä.
        // No niin ei toi suunnan kirjanpito toimi - jätä siis kääntyminen
        // tekemättä koska TÄLLÄ HETKELLÄ tietokoneen koodissa mittaussuunta
        // on joka tapauksessa kulkemissuuntaan päin...
        // pilotti.rotate(asettaja.getPose().angleTo(new Point(
        //     paketti.getMittausSuunta().x,
        //     paketti.getMittausSuunta().y)));

        /* Pyöritetään sensoritornia tasaisin astevälein.
         * Käännetään takaisin päin, kun torni on pyörinyt 90 astetta.
         * Otetaan arvot tutkalta robotille.*/
        int[] etaisyydet = tutka.haeArvot(paketti.getEtaisyydet().length);
        //laitetaan saadut mittaustulokset pakettiin
        paketti.setEtaisyydet(etaisyydet);
        
        //asetetaan paketin nykysijainniksi nykyinen sijainti
        //paketti.setNykySijainti(new Point2D.Float(asettaja.getPose().getX(),
          //      asettaja.getPose().getY()));
        // THIS BE PUUKKO.
        paketti.setNykySijainti(paketti.getUusiSijainti());
       // paketti.setMittausSuunta;
        
        return paketti;
    }
    
    public void testaaItsesi() {
        Button.waitForPress();
        BTPaketti paketti = new BTPaketti(0);
        paketti.setNykySijainti(new Point2D.Float(1000, 1000));
        paketti.setUusiSijainti(new Point2D.Float(1200, 1000));
        paketti.setMittausSuunta(new Point2D.Float(1200, 1200));
        paketti.setEtaisyydet(new int[5]);
        paketti = liikuJaMittaa(paketti);
        System.out.println("Self test done.\nPress button.");
    }
    
}
